#include "qs_config.h"
#include "qs_flash.h"
#define FLASH_USER_START_ADDR     0x08005F80



static __no_init qs_mem_data_t m_mem_data;

/**
  * @brief  Erase the user Flash area
  * @param  None
  * @retval None
  */


static void APP_FlashErase(void)
{
  uint32_t PAGEError = 0;
  FLASH_EraseInitTypeDef EraseInitStruct;

  EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGEERASE;                                     /* Macro FLASH_TYPEERASE_PAGEERASE means Page erase , Macro FLASH_TYPEERASE_SECTORERASE means Sector erase */
  EraseInitStruct.PageAddress = FLASH_USER_START_ADDR;                                         /* Start address of erase area */
  EraseInitStruct.NbPages  = 1;                                                                  /* Number of pages to be erased */
  if (HAL_FLASHEx_Erase(&EraseInitStruct, &PAGEError) != HAL_OK)                               /* Perform page erase,PAGEError save faulty page in case of error(0xFFFFFFFF means that all the pages have been correctly erased) */
  {
    APP_ErrorHandler();
  }
}
/**
  * @brief  Write FLASH
  * @param  None
  * @retval None
  */
static void APP_FlashProgram(void)
{
  uint32_t flash_program_start = FLASH_USER_START_ADDR ;                                /* Start address of user write flash */
  uint32_t flash_program_end = (FLASH_USER_START_ADDR + sizeof(m_mem_data));                  /* End address of user write flash */
  uint32_t *src = (uint32_t *)&m_mem_data;                                                     /* Pointer to array */
  
  while (flash_program_start < flash_program_end)
  {
    if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_PAGE, flash_program_start, src) == HAL_OK)  /* Perform Program */
    {
//      flash_program_start += FLASH_PAGE_SIZE;                                           /* Point to the start address of the next page to be written */
//      src += FLASH_PAGE_SIZE / 4;                                                       /* Point to the next data to be written */
//      
      flash_program_start += 4;                                           /* Point to the start address of the next page to be written */
      src ++;                                                       /* Point to the next data to be written */
    }
  }
}
/**
  * @brief  Check Flash if blank
  * @param  None
  * @retval None
  */
static void APP_FlashBlank(void)
{
  uint32_t addr = 0;

  while (addr < sizeof(m_mem_data))
  {
    if (0xFFFFFFFF != HW32_REG(FLASH_USER_START_ADDR + addr))
    {
      APP_ErrorHandler();
    }
    addr += 4;
  }
}


void qs_flash_mac_write(qs_mem_data_t* mac)
{
    memcpy(&m_mem_data, mac, sizeof(qs_mem_data_t));
      /* Unlock the Flash to enable the flash control register access */
    HAL_FLASH_Unlock();

  /* Erase Flash */
    APP_FlashErase();

  /* Check Flash if blank */
    APP_FlashBlank();

  /* Write user data to Flash */
    APP_FlashProgram();

  /* Lock the Flash to disable the flash control register access */
    HAL_FLASH_Lock();
}

qs_mem_data_t* qs_flash_mac_read(void)
{
    qs_mem_data_t *p_mem_data = (qs_mem_data_t *)FLASH_USER_START_ADDR;
    memcpy(&m_mem_data, p_mem_data, sizeof(qs_mem_data_t));
    return &m_mem_data;
}